
#include <iostream>
#include <stdint.h>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgproc/types_c.h>
#include <opencv2/core/types_c.h>
#include <opencv2/core/mat.hpp>







using namespace std;
using namespace cv;

#define PI (3.1415926)

int main()
{
    float tbc[3][3] = {1., 0., 0.,
                       0., 1., 0.,
                       0., 0., 1.,};

    cv::Mat tbc_mat = cv::Mat(cv::Size(3,3),CV_32FC1,tbc);

    std::cout << "tbc_mat: " << tbc_mat << std::endl;


	//fx fy cx cy，w
    float fx = 2.8346074536257589e+02;//param[0];
    float fy = 2.8425884788532824e+02;//param[1];
    float cx = 3.1526880119798028e+02;//param[2];
    float cy = 1.9923648982898769e+02;//param[3];

    float k1  = -2.0813913286177557e-02;//param[4];
    float k2  = 7.9698432208114497e-02;//param[5];

    float p1 = -0.0005570137397746864;
    float p2 = 0.0005840296330395519;

    float k3  = -1.0657364426048353e-01;//param[6];
    float k4  = 5.1052140487531254e-02;//param[7];



    int width = 640;
    int height = 400;


    
    float u = 2.0;
    float v = 0.0;
    float z = 0.05;
    
    float fov = 90 - atan2(z,u) / PI * 180;
    cout << "now fov: " << fov << endl;

    float norm = u*u + v*v + z*z;

    float p_test[3] = {0.,0.,0.};

    p_test[0] = u/norm;
    p_test[1] = v/norm;
    p_test[2] = z/norm;

    
   
    float u_test = p_test[0]*fx/p_test[2] + cx;
    float y_test = p_test[1]*fy/p_test[2] + cy;

    float xx_distorted = p_test[0]/p_test[2];
    float yy_distorted = p_test[1]/p_test[2];

    cout << "u_test: " << u_test << " v_test: " << y_test << endl;

    // return 0;
    // undistortion

    float r = sqrt(xx_distorted * xx_distorted + yy_distorted * yy_distorted);

    float x_distorted = xx_distorted*(1+k1*r*r+k2*r*r*r*r+k3*r*r*r*r*r*r) + 2*p1*xx_distorted*yy_distorted + p2*(r*r+2*xx_distorted*xx_distorted);
    float y_distorted = yy_distorted*(1+k1*r*r+k2*r*r*r*r+k3*r*r*r*r*r*r) + p1*(r*r+2*yy_distorted*yy_distorted)+2*p2*xx_distorted*yy_distorted;
    

    float u_distorted = fx*x_distorted + cx;
    float v_distorted = fy*y_distorted + cy;

    cout << "u_distorted: " << u_distorted << " v_distorted: " << v_distorted << endl;


}

